import re
import sys
import threading
import time

import pygame

output_mod = True

start_number = -1

# 最大速度设置
speed = 60
# 下降比率
land_rate = 0.06
# 速度上限
land_speed = 30


class Mission:

    def control_t(self):
        """
        自动任务线程
        :return:
        """
        while True:
            # if self.land_switch and self.on_fly:
            #
            #     if self.d >= 100:
            #         self.cmd_in.put(['down {}'.format(20), 1])
            #         self.cmd_out.get()
            #     if self.dx > 0:
            #         self.cmd_in.put(['back {}'.format(self.dx), 1])
            #     else:
            #         self.cmd_in.put(['forward {}'.format(-self.dx), 1])
            #     self.cmd_out.get()
            #     if self.dy > 0:
            #         self.cmd_in.put(['left {}'.format(self.dy), 1])
            #     else:
            #         self.cmd_in.put(['right {}'.format(-self.dy), 1])
            #     self.cmd_out.get()
            #     if -20 <= self.dx <= 20 and -20 <= self.dy <= 20:
            #         self.cmd_in.put(['land', 1])
            #         self.cmd_out.get()
            #         self.land_switch = False
            #         self.on_fly = False
            #         self.cmd_in.put('downvision {}'.format(0))
            #         print(" *** ", self.cmd_out.get())

            while self.control_switch:
                self.cmd_in.put(['takeoff', 1])
                self.cmd_out.get()
                time.sleep(0.5)
                self.cmd_in.put(['cw 90', 1])
                self.cmd_out.get()
                time.sleep(0.5)
                self.cmd_in.put(['ccw 90', 1])
                self.cmd_out.get()
                time.sleep(0.5)
                self. cmd_in.put(['ccw 90', 1])
                self. cmd_out.get()
                self. cmd_in.put(['go -60, -60, 80, 60, m12', 1])
                self. cmd_out.get()
                self.cmd_in.put(['land', 1])
                self. cmd_out.get()
                self.control_switch = False
                # if self.mission_numb == -1:
                #     print(1)
                #     # self.cmd_in.put(['takeoff', 1])
                #     # self.cmd_out.get()
                #     # self.cmd_in.put(['up 50', 1])
                #     # self.cmd_out.get()
                #     # self.on_fly = True
                #     self.mission_numb = 0
                # elif self.mission_numb == 0:
                #     print(1)
                #     self.cmd_in.put(['takeoff', 1])
                #     self.cmd_out.get()
                #     self.on_fly = True
                #     self.mission_numb = 1
                # elif self.mission_numb == 1:
                #     self.cmd_in.put(['up 100', 1])
                #     self.cmd_out.get()
                #     self.cross_circle()
                #     self.mission_numb = 2
                # elif self.mission_numb == 2:
                #     self.cmd_in.put(['up 80', 1])
                #     self.cmd_out.get()
                #     self.cross_circle()
                #     self.mission_numb = 3
                # elif self.mission_numb == 3:
                #     d = self.f_distance()
                #     while d > 2000:
                #         self.cmd_in.put(['forward {}'.format(20), 1])
                #         self.cmd_out.get()
                #         d = self.f_distance()
                #
                #     self.cmd_in.put(['forward {}'.format(int(d / 10) - 20), 1])
                #     self.cmd_out.get()
                #     self.mission_numb = 4
                # elif self.mission_numb == 4:
                #     self.cmd_in.put(['right 50', 1])
                #     self.cmd_out.get()
                #     self.cmd_in.put(['down 50', 1])
                #     self.cmd_out.get()
                #     self.mission_numb += 1
                # elif self.mission_numb == 5:
                #     self.cmd_in.put(['jump 170 0 80 60 0 m1 m2', 1])
                #     self.cmd_out.get()
                #     self.mission_numb += 1
                # elif self.mission_numb == 6:
                #     self.cmd_in.put(['jump 170 0 80 60 0 m2 m3', 1])
                #     self.cmd_out.get()
                #     self.cmd_in.put(['up 100', 1])
                #     self.cmd_out.get()
                #     self.mission_numb = 8
                # elif self.mission_numb == 8:
                #     time.sleep(2)
                #     self.cross_circle(f_type=1)
                #     self.mission_numb += 1
                # elif self.mission_numb == 9:
                #     self.cmd_in.put(['downvision {}'.format(1), 1])
                #     print(" *** ", self.cmd_in.get())
                #     self.control_switch = False
                #     self.mission_numb += 1

                # elif self.mission_numb == 9:
                #     self.cmd_in.put(['land', 1])
                #     self.cmd_out.get()
                #     self.mission_numb += 1
                # elif self.mission_numb == 2:
                #     self.land_switch = True
                #     self.mission_numb += 1

    mission_numb = 0

    # 自动任务开关
    control_switch = False
    control_thread = None
    # 自动降落开关
    land_switch = False

    on_fly = False

    roll = 0
    pitch = 0
    throttle = 0
    yaw = 0

    dx = 0
    dy = 0
    d = 0

    screen = None

    rx = -1
    ry = -1

    def update_res(self):
        """
        圆圈信息更新线程
        :return:
        """
        while True:
            rst = []
            while True:
                rst = self.rst.get()
                if self.rst.empty():
                    break

            self.dx = int(rst[0] - 5)
            self.dy = int(rst[1])
            self.d = int(rst[2])
            # print(self.dx, self.dy)

    def cross_circle(self, f_type=-1):
        if 0 < self.dx <= 20:
            self.cmd_in.put(['left {}'.format(20), 1])
            self.cmd_out.get()
        elif -20 < self.dx <= 0:
            self.cmd_in.put(['right {}'.format(20), 1])
            self.cmd_out.get()
        time.sleep(2)
        if self.dx > 0:
            self.cmd_in.put(['right {}'.format(self.dx), 1])
        else:
            self.cmd_in.put(['left {}'.format(-self.dx), 1])
        self.cmd_out.get()
        d = self.d
        if self.dy > 0:
            self.cmd_in.put(['down {}'.format(self.dy), 1])
        else:
            self.cmd_in.put(['up {}'.format(-self.dy), 1])
        self.cmd_out.get()
        if f_type == 1:
            self.cmd_in.put(['forward {}'.format(d + 100), 1])
        else:
            self.cmd_in.put(['forward {}'.format(d + 30), 1])
        self.cmd_out.get(5)

    def __init__(self, rst=None, cmd_in=None, cmd_out=None):
        self.f_d = 170
        self.c_time = None
        self.cot = 0
        self.rdy = 0
        self.rdx = 0
        self.rx = 0
        self.ry = 0
        self.rst = rst
        self.cmd_in = cmd_in
        self.cmd_out = cmd_out
        self.control_thread = threading.Thread(target=self.control_t)
        self.res_thread = threading.Thread(target=self.update_res)

    def start(self):
        # 初始化 Pygame
        self.res_thread.start()
        self.control_thread.start()

        pygame.init()

        # 设置窗口大小
        self.screen = pygame.display.set_mode((400, 300))
        while True:
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
                    sys.exit()
                elif event.type == pygame.KEYDOWN:
                    if event.key == pygame.K_w:
                        self.pitch = speed
                        self.send_new()
                        if output_mod:
                            print("按下了 w 键！")
                    elif event.key == pygame.K_a:
                        self.roll = -speed
                        self.send_new()
                        if output_mod:
                            print("按下了 a 键！")
                    elif event.key == pygame.K_s:
                        self.pitch = -speed
                        self.send_new()
                        if output_mod:
                            print("按下了 s 键！")
                    elif event.key == pygame.K_d:
                        self.roll = speed
                        self.send_new()
                        if output_mod:
                            print("按下了 d 键！")
                    elif event.key == pygame.K_SPACE:
                        self.throttle = speed
                        self.send_new()
                        if output_mod:
                            print("按下了空格键！")
                    elif event.key == pygame.K_LSHIFT or event.key == pygame.K_RSHIFT:
                        self.throttle = -speed
                        self.send_new()
                        if output_mod:
                            print("按下了 Shift 键！")
                    elif event.key == pygame.K_LEFT:
                        self.yaw = -100
                        self.send_new()
                        if output_mod:
                            print("按下了左箭头键！")
                    elif event.key == pygame.K_RIGHT:
                        self.yaw = 100
                        self.send_new()
                        if output_mod:
                            print("按下了右箭头键！")
                elif event.type == pygame.KEYUP:
                    if event.key == pygame.K_w:
                        self.pitch = 0
                        self.send_new()
                        if output_mod:
                            print("松开了 w 键！")
                    elif event.key == pygame.K_a:
                        self.roll = 0
                        self.send_new()
                        if output_mod:
                            print("松开了 a 键！")
                    elif event.key == pygame.K_s:
                        self.pitch = 0
                        self.send_new()
                        if output_mod:
                            print("松开了 s 键！")
                    elif event.key == pygame.K_d:
                        self.roll = 0
                        self.send_new()
                        if output_mod:
                            print("松开了 d 键！")
                    elif event.key == pygame.K_SPACE:
                        self.throttle = 0
                        self.send_new()
                        if output_mod:
                            print("松开了空格键！")
                    elif event.key == pygame.K_LSHIFT or event.key == pygame.K_RSHIFT:
                        self.throttle = 0
                        self.send_new()
                        if output_mod:
                            print("松开了 Shift 键！")
                    elif event.key == pygame.K_LEFT:
                        self.yaw = 0
                        self.send_new()
                        if output_mod:
                            print("松开了左箭头键！")
                    elif event.key == pygame.K_RIGHT:
                        self.yaw = 0
                        self.send_new()
                        if output_mod:
                            print("松开了右箭头键！")
                    elif event.key == pygame.K_t:
                        if not self.on_fly:
                            if output_mod:
                                print("松开了t键！")
                            self.cmd_in.put(['takeoff', 1])
                            self.cmd_out.get()
                            self.on_fly = True
                    elif event.key == pygame.K_y:
                        if self.on_fly:
                            if output_mod:
                                print("松开了y键！")
                            self.cmd_in.put(['land', 1])
                            self.cmd_out.get()
                            self.on_fly = False
                    elif event.key == pygame.K_o:
                        self.control_switch = False
                        if output_mod:
                            print("手动控制")
                    elif event.key == pygame.K_p:
                        self.mission_numb = start_number
                        self.control_switch = True
                        if output_mod:
                            print("自动控制")
                    elif event.key == pygame.K_g:
                        self.land_switch = True
                        if output_mod:
                            print("跟踪降落")

    def send_new(self):
        if not self.control_switch:
            if self.on_fly:
                cmd = ['rc {} {} {} {}'.format(self.roll, self.pitch, self.throttle, self.yaw), 0]
                if output_mod:
                    print(cmd)
                self.cmd_in.put(cmd)

    # 返回前方距离,单位mm
    def f_distance(self):
        s = "u"
        d = 0
        while s[0] == 'u' or d < 100:
            self.cmd_in.put(['EXT tof?', 1])
            s = self.cmd_out.get()
            d = re.findall("\d+\.?\d*", s)
            d = list(map(int, d))[0]
            print(d)
        return d
